﻿using System;
using System.Linq;
using System.Text;
using Microsoft.Xna.Framework;
using System.Collections;
using System.Collections.Generic;

namespace MobMentality.GraphUtil
{
	

	public class Graph<T>
	{
		public delegate float distanceHeuristic(T from, T to);

		private Dictionary<GraphNode<T>, Dictionary<GraphNode<T>, float>> graph;

		public Graph() : this(null) { }
		public Graph(Dictionary<GraphNode<T>, Dictionary<GraphNode<T>, float>> nodeSet)
		{
			if (nodeSet == null)
				this.graph = new Dictionary<GraphNode<T>, Dictionary<GraphNode<T>, float>>();
			else
				this.graph = nodeSet;
		}

		public void AddNode(GraphNode<T> node)
		{
			// adds a node to the graph
			graph.Add(node, new Dictionary<GraphNode<T>, float>());
		}

		public void AddNode(T value)
		{
			// adds a node to the graph
			graph.Add(new GraphNode<T>(value), new Dictionary<GraphNode<T>, float>());
		}

		public void AddEdge(T from, T to, float cost)
		{
			GraphNode<T> fromNode = graph.Keys.Single( node => node.Value.Equals(from) );
			GraphNode<T> toNode = graph.Keys.Single( node => node.Value.Equals(to) );
			graph[fromNode].Add(toNode, cost);
		}

		public bool Contains(T value)
		{
			return graph.Keys.Contains(new GraphNode<T>(value));
		}

		public void Remove(T value)
		{
			graph.Remove(new GraphNode<T>(value));
		}

		public Dictionary<GraphNode<T>, Dictionary<GraphNode<T>,float>>.KeyCollection Nodes
		{
			get
			{
				return graph.Keys;
			}
		}

		public int Count
		{
			get { return graph.Keys.Count; }
		}

		/*Not super efficient, but it's AStar*/
		public GraphNode<T> AStarNextNode(T startTile, T endTile, distanceHeuristic heuristic)
		{
			GraphNode<T> startNode = graph.Keys.Single( node => node.Value.Equals(startTile) );
			GraphNode<T> endNode = graph.Keys.Single( node => node.Value.Equals(endTile) );

			if (startNode.Value.Equals(endNode.Value))
			{
				return startNode;
			}

			if (!graph.Keys.Contains(startNode))
			{
				return null;
			}

			List<GraphNode<T>> closedSet = new List<GraphNode<T>>();
			List<GraphNode<T>> openSet = new List<GraphNode<T>>();
			openSet.Add(startNode);

			Dictionary<GraphNode<T>, GraphNode<T>> cameFrom = new Dictionary<GraphNode<T>,GraphNode<T>>();

			cameFrom[startNode] = null;
			startNode.GScore = 0;
			startNode.HScore = heuristic(startNode.Value, endNode.Value);
			startNode.FScore = startNode.GScore + startNode.HScore;

			while (openSet.Count != 0)
			{
				//what, a sorting function based on a comparison delegate? Why thank you, C#
				openSet.Sort(delegate(GraphNode<T> node1, GraphNode<T> node2) { return node1.FScore.CompareTo(node2.FScore); });
				GraphNode<T> current = openSet[0];

				if (current.Value.Equals(endNode.Value))
				{
					return reconstructPath(startNode, current, cameFrom);
				}

				openSet.Remove(current);
				closedSet.Add(current);

				foreach (GraphNode<T> neighbor in graph[current].Keys)
				{
					if (closedSet.Contains(neighbor))
					{
						continue;
					}

					float tentativeGScore = current.GScore + graph[current][neighbor];
					bool tentativeBetter = false;

					if (!openSet.Contains(neighbor))
					{
						openSet.Add(neighbor);
						tentativeBetter = true;
					}
					else if (tentativeGScore < neighbor.GScore)
					{
						tentativeBetter = true;
					}
					else
					{
						tentativeBetter = false;
					}

					if (tentativeBetter)
					{
						cameFrom[neighbor] = current;
						neighbor.GScore = tentativeGScore;
						neighbor.HScore = heuristic(neighbor.Value, endNode.Value);
						neighbor.FScore = neighbor.GScore + neighbor.HScore;
					}
				}
			}

			//no available path
			return null;
		}

		private GraphNode<T> reconstructPath(GraphNode<T> start, GraphNode<T> current, Dictionary<GraphNode<T>, GraphNode<T>> cameFrom)
		{
			if (cameFrom[current].Value.Equals(start.Value))
			{
				return current;
			}
			else
			{
				return reconstructPath(start, cameFrom[current], cameFrom);
			}
		}

		/*Not super efficient, but it's AStar*/
		public int AStarDistance(T startTile, T endTile, distanceHeuristic heuristic)
		{
			GraphNode<T> startNode = graph.Keys.Single(node => node.Value.Equals(startTile));
			GraphNode<T> endNode = graph.Keys.Single(node => node.Value.Equals(endTile));

			if (!graph.Keys.Contains(startNode))
			{
				//definitely larger than reachable nodes
				return graph.Count*graph.Count;
			}

			List<GraphNode<T>> closedSet = new List<GraphNode<T>>();
			List<GraphNode<T>> openSet = new List<GraphNode<T>>();
			openSet.Add(startNode);

			Dictionary<GraphNode<T>, GraphNode<T>> cameFrom = new Dictionary<GraphNode<T>, GraphNode<T>>();

			cameFrom[startNode] = null;
			startNode.GScore = 0;
			startNode.HScore = heuristic(startNode.Value, endNode.Value);
			startNode.FScore = startNode.GScore + startNode.HScore;

			while (openSet.Count != 0)
			{
				//what, a sorting function based on a comparison delegate? Why thank you, C#
				openSet.Sort(delegate(GraphNode<T> node1, GraphNode<T> node2) { return node1.FScore.CompareTo(node2.FScore); });
				GraphNode<T> current = openSet[0];

				if (current.Value.Equals(endNode.Value))
				{
					return (int)current.GScore;
				}

				openSet.Remove(current);
				closedSet.Add(current);

				foreach (GraphNode<T> neighbor in graph[current].Keys)
				{
					if (closedSet.Contains(neighbor))
					{
						continue;
					}

					float tentativeGScore = current.GScore + graph[current][neighbor];
					bool tentativeBetter = false;

					if (!openSet.Contains(neighbor))
					{
						openSet.Add(neighbor);
						tentativeBetter = true;
					}
					else if (tentativeGScore < neighbor.GScore)
					{
						tentativeBetter = true;
					}
					else
					{
						tentativeBetter = false;
					}

					if (tentativeBetter)
					{
						cameFrom[neighbor] = current;
						neighbor.GScore = tentativeGScore;
						neighbor.HScore = heuristic(neighbor.Value, endNode.Value);
						neighbor.FScore = neighbor.GScore + neighbor.HScore;
					}
				}
			}

			//no available path
			//definitely larger than reachable nodes
			return graph.Count * graph.Count;
		}
	}
}
